#!/usr/bin/env python3
# -*- coding: utf-8 -*-

# @Author: MingshanHe
# @Date: 2021-12-05 04:08:58
# @Last Modified by: MingshanHe
# @Last Modified time: 2021-12-05 04:13:10
# @Licence: MIT Licence

import rospy
from geometry_msgs.msg import WrenchStamped
from math import sin

WRENCH_TOPIC = "/wrench_fake"
TOPIC_HZ = 125.0

def main():
    rospy.init_node("wrench_signal_generate")
    wrench_pub = rospy.Publisher(WRENCH_TOPIC, WrenchStamped, queue_size=5)
    rospy.sleep(5.0)
    rate = rospy.Rate(TOPIC_HZ)
    wrench_msg = WrenchStamped()
    t = 0

    while not rospy.is_shutdown():
        if int(t) % 10 < 5:
            wrench_msg.wrench.force.z = 5
        else:
            wrench_msg.wrench.force.z = -5

        t += 1 / TOPIC_HZ
        wrench_pub.publish(wrench_msg)
        rate.sleep()

if __name__ == "__main__":
    main()
